/*
 * kalman.h
 *
 *  Created on: 10-12-2012
 *      Author: Maciej
 */

#ifndef KALMAN_H_
#define KALMAN_H_

struct s_KalmanFilter
{

	 float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise

	  float angle; // The angle calculated by the Kalman filter - part of the 2x1 state matrix
	  float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix
	  float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call Process to update the rate

	  // 0 - Process noise variance for the accelerometer based angle
	  // 1 - Process noise variance for the gyro bias
	  float Q[2];     // Process noise covariance

	  float P[2][2];   // Error covariance matrix - This is a 2x2 matrix
	  float K[2];        // Kalman gain - This is a 2x1 matrix
	  float y;                         // Angle difference - 1x1 matrix
	  float S;                         // Estimate error - 1x1 matrix

};

typedef struct s_KalmanFilter sKalmanFilter;


void Kalman_Init(sKalmanFilter* filter);

float Kalman_Process(sKalmanFilter* , float , float , float);

#endif /* KALMAN_H_ */
